/**
 * 用后轮反馈控制算法(rear_wheel_feedback)实现倒车的横向轨迹跟踪
 * @author:胡云浩
 * @email: hnu_huyunhao1995@163.com
 * */
#ifndef ROS_REARWHEEL_FEEDBACK_REARWHEEL_FEEDBACK_H
#define ROS_REARWHEEL_FEEDBACK_REARWHEEL_FEEDBACK_H

#include "my_functions.h"
#include "nlohmann/json.hpp"
#include <string>

namespace controlNS {
using namespace std;
// 定义/读取一些控制常数
extern nlohmann::json control_json; //存放控制参数的json配置文件

extern float kPI;   //定义pi
extern float kPhi;  //定义rearwheel_feedback算法控制参数：横摆角偏差增益
extern float kE;    //定义rearwheel_feedback算法控制参数：横向位置偏差增益
extern float kWheelBase; //定义车辆的轴距[m]
extern float kSteeringRatio;    //定义车辆的传动比
extern float kMaxSteerAngle;
class State {
    /*用于更新车辆实时状态的类*/
public:
    // 构造函数初始化车辆一开始的停放位置
    explicit State(float x = 0, float y = 0, float yaw = 0, float v = 0) {
        this->x_ = x;
        this->y_ = y;
        this->yaw_ = myNumpy::NormalizeAngle(yaw);  //将yaw angle的角度限制在[-pi, pi]之间
        this->v_ = v;
    };

    // 接收来自PreScan的车辆实时状态信息
    void updateFromPreScan(float x, float y, float yaw, float v);

public:
    float x_;   //车辆的全局x坐标[m]
    float y_;   //车辆的全局y坐标[m]
    float yaw_; //车辆的横摆角[rad]
    float v_;   //车辆的实时速度[m/s]
};

class CalcPath {
    /**
     * @describe: 计算决策下发的路径信息的类
     * 计算离散点的一阶导数,二阶导数,曲率等。
     * */
public:
    CalcPath(const string &path_x, const string &path_y, const string &path_yaw, const State &state);

    void calNearestPoint(const State &state);    //求路径上距离后轮中心的最近点坐标的索引
    void calDiff1();
    void calDiff2();
    void calCurvature();
    void calError(const State& state);
    void calUpdate(const State& state);

public:
    vector<float> path_x_;  //决策下发的路径的x坐标点集
    vector<float> path_y_;
    vector<float> path_yaw_;
    int min_distance_index_;    //路径上距离车辆后轮中心最近点的索引
    float cross_track_error_;   //横向偏差[m]
    float heading_track_error_; //航向偏差[deg]
    float diff1_;               //最近点的一阶导数
    float diff2_;               //最近点的二阶导数
    float curvature_;           //最近点的曲率
};

class RearWheelFeedbackControl{
public:
    RearWheelFeedbackControl() = default;
    float calDelta(const State& state, const CalcPath& calcPath);
public:
    float delta_;   //输出的方向盘转角[deg]
};

}; // namespace controlNS;
#endif //ROS_REARWHEEL_FEEDBACK_REARWHEEL_FEEDBACK_H
